/*
 *  test.cpp
 *  bgsubtraction
 *
 *  Created by a1gucis on 3/17/09.
 *  Copyright 2009 __MyCompanyName__. All rights reserved.
 *
 */

#include "test.h"
#define NUM_CLUSTERS 4
#define FRAMES_TO_TRAIN_EM 3
const CvScalar CVX_WHITE = CV_RGB(0xff,0xff,0xff); 
const CvScalar CVX_BLACK = CV_RGB(0x00,0x00,0x00);

void Test::Run() {
	FgFiltering *fgFiltering = new FgFiltering(FRAME_HEIGHT, FRAME_WIDTH);
	CodeBookModel *model = new CodeBookModel(FRAME_HEIGHT, FRAME_WIDTH);
	EM *em = new EM();
	MRF *mrf = new MRF();
	//capture from camera
	//CvCapture *capture = cvCaptureFromCAM(0);

	//cvSetCaptureProperty( capture, CV_CAP_PROP_FPS, 1 );
	//cvSetCaptureProperty( capture, CV_CAP_PROP_FRAME_WIDTH, 320 );
	

	//capture from video file
	CvCapture *capture = cvCreateFileCapture("./video.avi");
	if (capture == NULL) {
		cout<<"Error with capture"<<endl;
		exit (-1);
	}
	cvNamedWindow("Raw", CV_WINDOW_AUTOSIZE);
	cvNamedWindow("Clustered", CV_WINDOW_AUTOSIZE);
	cvNamedWindow("MRF", CV_WINDOW_AUTOSIZE);
	//cvNamedWindow("FG", CV_WINDOW_AUTOSIZE);
	IplImage *rawImg = cvQueryFrame(capture);
	IplImage *yuvImg = cvCreateImage(cvGetSize(rawImg), IPL_DEPTH_8U, 3);
	IplImage *fgImg = cvCreateImage(cvGetSize(rawImg), IPL_DEPTH_8U, 1);
	IplImage *fgFiltered = cvCreateImage(cvGetSize(rawImg), IPL_DEPTH_8U, 1);
	IplImage *trueForeground = cvCreateImage(cvGetSize(rawImg), IPL_DEPTH_8U, 3);
	IplImage *cropedForeground = NULL;
	IplImage *clustered = NULL;
	IplImage *clusteredLabels = NULL;
	IplImage *mrfOutput = NULL;
	CvScalar boundingRect;
	int framesLearned = 0;
	char c;
	bool record = false;
	int emTrain = 0;

	int fpsCounter = 1;

	while (1) {
		rawImg = cvQueryFrame(capture);
		cvSmooth(rawImg, rawImg, CV_MEDIAN, 5);
		//cvFlip(rawImg, NULL, 1);
		cvShowImage("Raw", rawImg);
		c = cvWaitKey(25);
		if (c == 'r')
			record = true;
		if (c == 27)
			break;
		if (!record) //do not record until r key is pressed
			continue;
		//cvCvtColor(rawImg, yuvImg, CV_BGR2HSV);
		cvCopy(rawImg, yuvImg);
		if (framesLearned < FRAMES_TO_LEARN) {
			framesLearned++;
			model->updateBG(yuvImg);
			if (framesLearned == FRAMES_TO_LEARN) {
				model->clearStale();
				cout<<"BACKGROUND LEARNING DONE"<<endl;
			}
			//start using the learned data to find the foreground
		}else {

			/*if(fpsCounter % 20 != 0)
			{
				fpsCounter++;
				continue;
			}
			fpsCounter = 1;*/

			model->bgDiff(yuvImg, fgImg);
			cvCopy(fgImg, fgFiltered);
			int numCont = fgFiltering->findConnectedComponents(fgFiltered, boundingRect);
			if (numCont > 0) {
				fgFiltering->segregateTrueForeground(fgFiltered, yuvImg, trueForeground);
		

				int height = boundingRect.val[3] - boundingRect.val[1];
				int width = boundingRect.val[2] - boundingRect.val[0];		

				cvSetImageROI(trueForeground, cvRect(boundingRect.val[0], boundingRect.val[1], width, height));
				if (cropedForeground != NULL)
					cvReleaseImage(&cropedForeground);
				
				cropedForeground = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 3);
				cvCopy(trueForeground, cropedForeground);
				cvResetImageROI(trueForeground);
				//cvShowImage("FG", cropedForeground);
				cvCvtColor(cropedForeground, cropedForeground, CV_BGR2HSV);
				//cvShowImage("MRF", trueForeground);
			//}
				if (emTrain < FRAMES_TO_TRAIN_EM) {
					em->init(cropedForeground->height, cropedForeground->width);
					em->train(cropedForeground, true);
					emTrain++;
					cout<<"EM_TRAIN: "<<emTrain<<endl;
					if (emTrain == FRAMES_TO_TRAIN_EM)
					{
						cout<<"EM TRAINING DONE"<<endl;

						const CvMat *means = em->get_means();
						cout<<"MEANS: "<<endl;
						Utils::PrintMat(means);
						
						const CvMat **covs = em->get_covs();
						cout<<"COVS: "<<endl;
						for (int i=0;i<NUM_CLUSTERS;i++) {
							Utils::PrintMat(covs[i]);
							cout<<"#################"<<endl;
						}
						mrf->setMeansAndVariances(means, covs);

					}
				}
				else  {

					// Avoid big images
					/*if (cropedForeground->height > 400 || cropedForeground->width > 400)
					{
						cout<<"Too big img"<<endl;
						continue;
					}*/

					//cout<<"PREDICTION"<<endl;
					if (clustered != NULL)
						cvReleaseImage(&clustered);
					clustered = cvCreateImage(cvGetSize(cropedForeground), IPL_DEPTH_8U, 1);
					em->init(cropedForeground->height, cropedForeground->width);
					em->predict(cropedForeground, clustered);
					//clusteredLabels = clustered;
					clusteredLabels = cvCloneImage(clustered);
					Utils::ToNormalImg(clustered, NUM_CLUSTERS);
					cvShowImage("Clustered", clustered);

					//mrf->initLearningMRF(clustered);
				//	mrf->initFastMRF(cropedForeground, clusteredLabels);
				//	mrfOutput = mrf->getLabelledImage();
					//Utils::ToNormalImg(mrfOutput, NUM_CLUSTERS);
				//	cvShowImage("MRF", mrfOutput);

					//Utils::ToNormalImg(clustered, NUM_CLUSTERS);
					//cvShowImage("Clustered", clustered);
				
				}
			}
		}
	}
	cvReleaseImage(&mrfOutput);
	cvReleaseImage(&cropedForeground);
	cvReleaseImage(&yuvImg);
	cvReleaseImage(&fgImg);
	cvReleaseImage(&fgFiltered);
	cvReleaseImage(&trueForeground);
	cvReleaseImage(&clustered);
	cvDestroyWindow("MRF");
	cvDestroyWindow("Raw");
	cvDestroyWindow("Clustered");
	
}
/*void Test::Run2() {
	cvNamedWindow("Original", CV_WINDOW_AUTOSIZE);
	cvNamedWindow("Clustered", CV_WINDOW_AUTOSIZE);
	IplImage *grayscale = cvLoadImage("test.jpg");
	//IplImage *grayscale = cvCreateImage(cvGetSize(raw), IPL_DEPTH_8U, 1);
	//cvCvtColor(raw, grayscale, CV_BGR2GRAY);
	
	IplImage *scratch = cvCreateImage(cvGetSize(grayscale), IPL_DEPTH_8U, 1);
	//cvConvertScale(grayscale, scratch, 1.0);
	
	cvShowImage("Original", grayscale);
	EM emTest();
	emTest.train(grayscale);
	cout<<"training ok"<<endl;
	emTest.predict(grayscale, scratch);
	cout<<"OK HERE"<<endl;
	MRF *mrf = new MRF();
//	mrf->initLearningMRF(img);
	mrf->initFastMRF(grayscale, scratch);
	cout<<"OK HERE2"<<endl;
	mrf->ouputLabelledImage();
	
	/*uchar *data = (uchar *)scratch->imageData;
	int step = scratch->widthStep;
	for (int i=0;i<scratch->height;i++) {
		for (int j=0;j<scratch->width;j++) {
			if (data[i*step+j] != 0)
				data[i*step+j] = 255;
		}
	}
	
	//IplImage *clustered = emTest.run2(grayscale, 2);
	//	IplImage *clustered = emTest.kMeans(grayscale, 2);
	cvShowImage("Clustered", scratch);
	while (1) {
		char c = cvWaitKey(25);
		if (c == 27)
			break;
	}
	cvReleaseImage(&grayscale);
	cvReleaseImage(&scratch);
	//cvReleaseImage(&raw);
	cvDestroyWindow("Original");
	cvDestroyWindow("Clustered");
	
}*/

void Test::Run3() {
	HumanModel *model = new HumanModel();
	IplImage *image = cvLoadImage("./simpson2.jpg");
	cvNamedWindow("Output", CV_WINDOW_AUTOSIZE);
	cvNamedWindow("Test", CV_WINDOW_AUTOSIZE);
	
	IplImage *head = model->getHeadImage(image);
	cvShowImage("Output", image);
	cvShowImage("Test", head);
	while (1) {
		char c = cvWaitKey(25);
		if (c == 27)
			break;
	}
	
	cvReleaseImage(&image);
	cvReleaseImage(&head);
	cvDestroyWindow("Output");
	cvDestroyWindow("Test");
}

void Test::Run4() {
	BodyPart bodyPart(3);
	bodyPart.test();
}

void Test::Run5() {
	HumanModel *model = new HumanModel();
	IplImage *image = cvLoadImage("./simpson2.jpg");
	IplImage *headImage = model->getHeadImage(image);
	cvNamedWindow("Output", CV_WINDOW_AUTOSIZE);
	cvShowImage("Output", headImage);
	
	Head *head = new Head();
	head->setOriginalImage(headImage);
	head->train(false);
	
	while (1) {
		char c = cvWaitKey(25);
		if (c == 27)
			break;
	}
	cvReleaseImage(&image);
	cvReleaseImage(&headImage);
	cvDestroyWindow("Output");
	
}

void Test::Run6() {
	ClusterFusion *clusterFusion = new ClusterFusion();
	clusterFusion->test();
}

void Test::Run7() {
	HumanModel *model = new HumanModel();
	IplImage *image = cvLoadImage("./snapshot.png");
	//IplImage *image = cvLoadImage("./simpson2Gray.jpg");
	//IplImage *imageGrayscale = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
	//cvCvtColor(image, imageGrayscale, CV_BGR2GRAY);
	//cvCvtColor(imageGrayscale, image, CV_GRAY2BGR);
	IplImage *headImage = model->getHeadImage(image);

	cvNamedWindow("Original", CV_WINDOW_AUTOSIZE);
	cvNamedWindow("UpperBody", CV_WINDOW_AUTOSIZE);
	cvNamedWindow("AfterLabeling", CV_WINDOW_AUTOSIZE);

	Head *head = new Head();
	head->setOriginalImage(headImage);
	head->train(true);
	head->setLabels();
	IplImage *labeled = head->getLabeledImage();
	int hairLabel = head->findHairLabel();
	int faceLabel = head->findFaceLabel();
	Utils::ToNormalImg(labeled, 3);
	cvShowImage("AfterLabeling", labeled);
	cvShowImage("Original", image);
	cout<<"HairLabel: "<<hairLabel<<endl;
	cout<<"FaceLabel: "<<faceLabel<<endl;
	
	IplImage *upperBodyImage = model->getUpperBodyImage(image);
	UpperBody *upperBody = new UpperBody();
	upperBody->setOriginalImage(upperBodyImage);
	upperBody->train(true);
	upperBody->setLabels();
	IplImage *upperBodyLabeled = upperBody->getLabeledImage();
	Utils::ToNormalImg(upperBodyLabeled, 3);
	cvShowImage("UpperBody", upperBodyLabeled);
	while (1) {
		char c = cvWaitKey(25);
		if (c == 27)
			break;
	}
	cvReleaseImage(&image);
	cvReleaseImage(&headImage);
	cvDestroyWindow("AfterLabeling");
	cvDestroyWindow("Original");	
	cvDestroyWindow("UpperBody");
}

void Test::Run8() {
	IplImage *image = cvLoadImage("./afterLabeling.jpg", CV_LOAD_IMAGE_GRAYSCALE);
	cvNamedWindow("Output", CV_WINDOW_AUTOSIZE);
	IplImage *mask = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
	
	static CvMemStorage*   mem_storage = NULL; 
	static CvSeq*          contours    = NULL; 
	//CLEAN UP RAW MASK 
	// 
	//cvMorphologyEx( image, image, 0, 0, CV_MOP_OPEN,  4 ); 
	cvMorphologyEx( image, image, 0, 0, CV_MOP_CLOSE, 5 ); 
	cvCanny(image, mask, 5, 30, 3);

	//FIND CONTOURS AROUND ONLY BIGGER REGIONS 
	// 
	
	/*if( mem_storage==NULL ) { 
		mem_storage = cvCreateMemStorage(0); 
	} else { 
		cvClearMemStorage(mem_storage); 
	} 
	cvFindContours(mask, mem_storage, &contours, sizeof(CvContour), CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
	//CvContourScanner scanner = cvStartFindContours(mask, mem_storage, sizeof(CvContour), CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE); 
	CvSeq* c = contours; 
	IplImage *contoursImage = cvCreateImage(cvGetSize(mask), IPL_DEPTH_8U, 1);
	cvZero(contoursImage);
	
	while (c != NULL) {
		
		char ch = cvWaitKey(25);
		if (ch == 27)
			break;
		if (ch != 'n')
			continue;
		 cvZero(contoursImage);

		 
		CvSeq *c_new = cvApproxPoly(c, sizeof(CvContour), mem_storage, CV_POLY_APPROX_DP, 1, 0); 

		cvDrawContours(contoursImage, c_new, CVX_WHITE, CVX_BLACK, -1, CV_FILLED, 8);
		//cvDrawContours(mask, c, CVX_WHITE, CVX_BLACK, -1, CV_FILLED, 8); 

			c = c->h_next;
		
	} 
	*/
	cvShowImage("Output", image);

	cout<<"contour writing done"<<endl;
	//cvShowImage("Output", mask);
	while (true) {
		char c = cvWaitKey(25);
		if (c == 27)
			break;
	}
	cvReleaseImage(&mask);
	cvReleaseImage(&image);
//	cvReleaseImage(&contoursImage);
	cvDestroyWindow("Output");
}

void Test::Run9() {
	IplImage *originalRgb = cvLoadImage("./snapshot.png");
	IplImage *original = cvCreateImage(cvGetSize(originalRgb), IPL_DEPTH_8U, 3);
	cvCvtColor(originalRgb, original, CV_BGR2YCrCb);
	FullBody *fullBody = new FullBody();
	fullBody->setOriginalImage(original);
	fullBody->train(true);
	fullBody->setLabels();
	
	IplImage *fullBodyLabels = fullBody->getLabeledImage();
	Utils::ToNormalImg(fullBodyLabels, NUMBER_OF_CLUSTERS);
	IplImage *mask = cvCreateImage(cvGetSize(fullBodyLabels), IPL_DEPTH_8U, 1);
	//cvCanny(fullBodyLabels, mask, 5, 30, 3);
	cvNamedWindow("Labeled", CV_WINDOW_AUTOSIZE);
	//cvShowImage("Labeled", mask);
	cvShowImage("Labeled", fullBodyLabels);
	while(true) {
		char c = cvWaitKey(25);
		if (c == 27)
			break;
	}
	
	cvDestroyWindow("Labeled");
}

void Test::Run10() {
	CvCapture *capture = cvCreateFileCapture("./video.avi");
	if (!capture) {
		cout<<"Capture error"<<endl;
		return;
	}
	FullBody *fullBody = new FullBody();
	IDGenerator *generator = new IDGenerator();
	vector<Blob *> *blobVector = new vector<Blob *>;
	IplImage *rgbImage8U = cvQueryFrame(capture);
	IplImage *labels8U = cvCreateImage(cvGetSize(rgbImage8U), IPL_DEPTH_8U, 1);
	IplImage *labelsCopy8U = cvCreateImage(cvGetSize(rgbImage8U), IPL_DEPTH_8U, 1);
	IplImage *hlsImage8U = cvCreateImage(cvGetSize(rgbImage8U), IPL_DEPTH_8U, 3);
	IplImage *hlsImage32F = cvCreateImage(cvGetSize(rgbImage8U), IPL_DEPTH_32F, 3);
	IplImage *hlsWeightedImage32F = cvCreateImage(cvGetSize(rgbImage8U), IPL_DEPTH_32F, 3);
	IplImage *blobImage = NULL;
	//IplImage *xyzFrame = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 3);
	cvNamedWindow("Labeled", CV_WINDOW_AUTOSIZE);
	cvNamedWindow("Original", CV_WINDOW_AUTOSIZE);
	cvNamedWindow("AfterEm", CV_WINDOW_AUTOSIZE);
	int frameNumber = 0;
	while (true) {
		char c = cvWaitKey(25);
		if (c == 27)
			break;
		if (c != 'n')
			continue;		
		rgbImage8U = cvQueryFrame(capture);
		if (rgbImage8U == NULL)
			break;
		cvCvtColor(rgbImage8U, rgbImage8U, CV_BGR2RGB);
		//cvCvtColor(frame, xyzFrame, CV_BGR2YCrCb);
		//cvCvtColor(frame, xyzFrame, CV_RGB2YCrCb);
		//cvCvtColor(frame, xyzFrame, CV_BGR2HSV);
		//cvCvtColor(frame, xyzFrame8U, CV_RGB2HSV);
		//cvCvtColor(frame, xyzFrame, CV_RGB2XYZ);
		cvShowImage("Original", rgbImage8U);
		Utils::ToHSIImg(rgbImage8U, hlsImage32F);
		Utils::ToWeightedImg32F(hlsImage32F, hlsWeightedImage32F);
		Utils::CheckForNans(hlsImage32F);		
		fullBody->setOriginalImage(hlsWeightedImage32F);
		fullBody->setLabels();
		int bgLabel = fullBody->getBgLabel();
		int fgArea = fullBody->getArea();
		cout<<"bgLabel: "<<bgLabel<<" fgArea: "<<fgArea<<endl;
		generator->reset();
		blobVector->clear();
		Blob *bgBlob = new Blob(generator->getNewID());
		bgBlob->setLabel(bgLabel);
		blobVector->push_back(bgBlob);
			
		labels8U = fullBody->getLabeledImage();
		cvCopy(labels8U, labelsCopy8U);
		Utils::ToNormalImg(labelsCopy8U, NUMBER_OF_CLUSTERS);
		cvShowImage("AfterEm", labelsCopy8U);

			//IplImage *fullBodyLabelsCopy = cvCreateImage(cvGetSize(fullBodyLabels), IPL_DEPTH_8U, 1);
			//cvCopy(fullBodyLabels, fullBodyLabelsCopy);
		if (blobImage != NULL)
			cvReleaseImage(&blobImage);
		blobImage = cvCreateImage(cvGetSize(labels8U), IPL_DEPTH_8U, 3);
		Blob::gatherBlobs(blobVector, labels8U, bgLabel, generator, fullBody->getOriginalImage(), blobImage);
		Blob::gatherNeighbours(blobVector, labels8U, bgLabel, blobImage);
		Blob::doBlobFusion(&blobVector, fullBody->getOriginalImage(), fgArea,  bgLabel);
		IplImage *blobOutputImage = cvCreateImage(cvGetSize(rgbImage8U), IPL_DEPTH_8U, 1);
		Utils::FillImg(blobOutputImage, 255);
		Test::drawBlobs(blobOutputImage, blobVector);
		Utils::ToNormalImg(blobOutputImage, NUMBER_OF_CLUSTERS);
		cvShowImage("Labeled", blobOutputImage);
		cvReleaseImage(&blobOutputImage);
		
		cout<<"FRAME: "<<++frameNumber<<endl;
	}
	blobVector->clear();
	cvReleaseImage(&blobImage);
	cvReleaseCapture(&capture);
	cvDestroyWindow("Labeled");
	cvDestroyWindow("Original");
	cvDestroyWindow("AfterEm");
	
}

void Test::Run11() {
	IplImage *rgbImage8U = cvLoadImage("./snapshot.png");
	FullBody *fullBody = new FullBody();
	IDGenerator *generator = new IDGenerator();
	vector<Blob *> *blobVector = new vector<Blob *>;
	IplImage *labels8U = cvCreateImage(cvGetSize(rgbImage8U), IPL_DEPTH_8U, 1);
	IplImage *labelsCopy8U = cvCreateImage(cvGetSize(rgbImage8U), IPL_DEPTH_8U, 1);
	IplImage *hlsImage8U = cvCreateImage(cvGetSize(rgbImage8U), IPL_DEPTH_8U, 3);
	IplImage *hlsImage32F = cvCreateImage(cvGetSize(rgbImage8U), IPL_DEPTH_32F, 3);
	IplImage *hlsWeightedImage32F = cvCreateImage(cvGetSize(rgbImage8U), IPL_DEPTH_32F, 3);
	IplImage *blobImage = NULL;
	//IplImage *xyzFrame = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 3);
	cvNamedWindow("Labeled", CV_WINDOW_AUTOSIZE);
	cvNamedWindow("Original", CV_WINDOW_AUTOSIZE);
	cvNamedWindow("AfterEm", CV_WINDOW_AUTOSIZE);
	cvCvtColor(rgbImage8U, rgbImage8U, CV_BGR2RGB);
		//cvCvtColor(frame, xyzFrame, CV_BGR2YCrCb);
		//cvCvtColor(frame, xyzFrame, CV_RGB2YCrCb);
		//cvCvtColor(frame, xyzFrame, CV_BGR2HSV);
		//cvCvtColor(frame, xyzFrame8U, CV_RGB2HSV);
		//cvCvtColor(frame, xyzFrame, CV_RGB2XYZ);
	cvShowImage("Original", rgbImage8U);
	Utils::ToHSIImg(rgbImage8U, hlsImage32F);
	Utils::ToWeightedImg32F(hlsImage32F, hlsWeightedImage32F);
	Utils::CheckForNans(hlsImage32F);
	//printValue8U(rgbImage8U, 106, 156);
	fullBody->setOriginalImage(hlsWeightedImage32F);
	//printValue32F(hlsWeightedImage32F, 106, 156);
	fullBody->setLabels();
	int bgLabel = fullBody->getBgLabel();
	int fgArea = fullBody->getArea();
	cout<<"bgLabel: "<<bgLabel<<" fgArea: "<<fgArea<<endl;
	generator->reset();
	blobVector->clear();
	Blob *bgBlob = new Blob(generator->getNewID());
	bgBlob->setLabel(bgLabel);
	blobVector->push_back(bgBlob);
		
	labels8U = fullBody->getLabeledImage();
	cvCopy(labels8U, labelsCopy8U);
	Utils::ToNormalImg(labelsCopy8U, NUMBER_OF_CLUSTERS);
	cvShowImage("AfterEm", labelsCopy8U);
		
		//IplImage *fullBodyLabelsCopy = cvCreateImage(cvGetSize(fullBodyLabels), IPL_DEPTH_8U, 1);
		//cvCopy(fullBodyLabels, fullBodyLabelsCopy);
	if (blobImage != NULL)
		cvReleaseImage(&blobImage);
	blobImage = cvCreateImage(cvGetSize(labels8U), IPL_DEPTH_8U, 3);
	//printValue32F(fullBody->getOriginalImage(), 106, 156);
	Blob::gatherBlobs(blobVector, labels8U, bgLabel, generator, fullBody->getOriginalImage(), blobImage);
	Blob::gatherNeighbours(blobVector, labels8U, bgLabel, blobImage);
	Blob::doBlobFusion(&blobVector, fullBody->getOriginalImage(), fgArea, bgLabel);
	IplImage *blobOutputImage = cvCreateImage(cvGetSize(rgbImage8U), IPL_DEPTH_8U, 1);
	Utils::FillImg(blobOutputImage, 255);
	Test::drawBlobs(blobOutputImage, blobVector);
	Utils::ToNormalImg(blobOutputImage, NUMBER_OF_CLUSTERS);
	cvShowImage("Labeled", blobOutputImage);
	IplImage *blobOutputImageCopy = cvCreateImage(cvGetSize(blobOutputImage), IPL_DEPTH_8U, 1);
	cvCopy(blobOutputImage, blobOutputImageCopy);
	
	int blobNr = 1;
	while (true) {
		char c = cvWaitKey(25);
		if (c == 27)
			break;
		if (c == 'n') {
			blobNr++;
			if (blobNr >= blobVector->size())
				blobNr--;
			cvReleaseImage(&blobOutputImageCopy);
			blobOutputImageCopy = cvCreateImage(cvGetSize(blobOutputImage), IPL_DEPTH_8U, 1);
			cvCopy(blobOutputImage, blobOutputImageCopy);
			cout<<"|"<<blobNr<<"|";
			Test::drawBlob(blobOutputImageCopy, (*blobVector)[blobNr], 0);

		}
		if (c == 'b') {
			blobNr--;
			if (blobNr < 1)
				blobNr++;
			cvReleaseImage(&blobOutputImageCopy);
			blobOutputImageCopy = cvCreateImage(cvGetSize(blobOutputImage), IPL_DEPTH_8U, 1);
			cvCopy(blobOutputImage, blobOutputImageCopy);
			cout<<"|"<<blobNr<<"|";
			Test::drawBlob(blobOutputImageCopy, (*blobVector)[blobNr], 0);

		}
		cvShowImage("Labeled", blobOutputImageCopy);
	}
	cvReleaseImage(&blobOutputImage);
	blobVector->clear();
	cvReleaseImage(&blobImage);
	cvDestroyWindow("Labeled");
	cvDestroyWindow("Original");
	cvDestroyWindow("AfterEm");
}

void Test::Run12() {
	IDGenerator *generator = new IDGenerator();
	Blob *blob = new Blob(generator->getNewID());
	vector<Blob *> *vectorOne = new vector<Blob *>;
	vector<Blob *> *vectorTwo = new vector<Blob *>;
	vectorOne->push_back(blob);
	vectorTwo->push_back(blob);
	
	Blob *nextBlob = (*vectorTwo)[0];
	nextBlob->setFused();
	vectorTwo->clear();
	Blob *oldBlob = (*vectorOne)[0];
	if (oldBlob->isFused())
		cout<<"OLD BLOB IS FUSED"<<endl;
	else
		cout<<"NOT FUSED"<<endl;
}

void Test::Run13() {
	IplImage *rgbImage8U = cvLoadImage("./snapshot2.bmp");
	IplImage *hlsImage8U = cvCreateImage(cvGetSize(rgbImage8U), IPL_DEPTH_8U, 3);
	IplImage *hlsImage32F = cvCreateImage(cvGetSize(rgbImage8U), IPL_DEPTH_32F, 3);
	cvNamedWindow("Output", CV_WINDOW_AUTOSIZE);
	float wChan1 = 1.0;
	float wChan2 = 1.0;
	float wChan3 = 1.0;
	while (true) {
		bool changesMade = false;
		char c = cvWaitKey(25);
		if (c == 27)
			break;
		switch(c) {
			case 'B':
				wChan1 += 0.1;
				changesMade = true;
				break;
			case 'N':
				wChan2 += 0.1;
				changesMade = true;
				break;
			case 'M':
				wChan3 += 0.1;
				changesMade = true;
				break;
			case 'b':
				wChan1 -= 0.1;
				changesMade = true;
				break;
			case 'n':
				wChan2 -= 0.1;
				changesMade = true;
				break;
			case 'm':
				wChan3 -= 0.1;
				changesMade = true;
				break;
		}
		cvCvtColor(rgbImage8U, hlsImage8U, CV_RGB2HLS);
		cvCvtScale(hlsImage8U, hlsImage32F, 1.0);
		uchar *data = (uchar *)hlsImage8U->imageData;
		for (int i=0;i<hlsImage32F->height;i++) {
			for (int j=0;j<hlsImage32F->width;j++) {
				float chan1 = ((float *)(hlsImage32F->imageData + i*hlsImage32F->widthStep))[j*hlsImage32F->nChannels+0];
				float chan2 = ((float *)(hlsImage32F->imageData + i*hlsImage32F->widthStep))[j*hlsImage32F->nChannels+1];
				float chan3 = ((float *)(hlsImage32F->imageData + i*hlsImage32F->widthStep))[j*hlsImage32F->nChannels+2];
				chan1 *= wChan1;
				chan2 *= wChan2;
				chan3 *= wChan3;
				//char h = floor(chan1);
				data[i*hlsImage8U->widthStep + j*hlsImage8U->nChannels + 0] = (char)round(chan1);
				data[i*hlsImage8U->widthStep + j*hlsImage8U->nChannels + 1] = (char)round(chan2);
				data[i*hlsImage8U->widthStep + j*hlsImage8U->nChannels + 2] = (char)round(chan3);
			}
		}
		if (changesMade)
			cout<<wChan1<<" "<<wChan2<<" "<<wChan3<<endl;
		cvShowImage("Output", hlsImage8U);
		
	}
	
	cvDestroyWindow("Output");
	cvReleaseImage(&rgbImage8U);
	cvReleaseImage(&hlsImage8U);
	cvReleaseImage(&hlsImage32F);
}

void Test::Run14() {
	IplImage *rgbImage8U = cvLoadImage("./snapshot2.bmp");
	IplImage *rgbImage32F = cvCreateImage(cvGetSize(rgbImage8U), IPL_DEPTH_32F, 3);
	IplImage *labels8U = cvCreateImage(cvGetSize(rgbImage8U), IPL_DEPTH_8U, 1);
	IplImage *hlsImage8U = cvCreateImage(cvGetSize(rgbImage8U), IPL_DEPTH_8U, 3);
	IplImage *hlsImage32F = cvCreateImage(cvGetSize(rgbImage8U), IPL_DEPTH_32F, 3);
	IplImage *hlsWeightedImage32F = cvCreateImage(cvGetSize(rgbImage8U), IPL_DEPTH_32F, 3);
	cvCvtColor(rgbImage8U, rgbImage8U, CV_BGR2RGB);
	Utils::ToHSIImg(rgbImage8U, hlsImage32F);
	Utils::ToWeightedImg32F(hlsImage32F, hlsWeightedImage32F);
	Utils::CheckForNans(hlsImage32F);

	KMeans *kmeans = new KMeans(4);
	kmeans->predict(hlsWeightedImage32F, labels8U);
	Utils::ToNormalImg(labels8U, 4);
	cvNamedWindow("Output", CV_WINDOW_AUTOSIZE);
	while (true) {
		char c = cvWaitKey(25);
		if (c == 27)
			break;
		cvShowImage("Output", labels8U);
	}
	cvDestroyWindow("Output");
	cvReleaseImage(&rgbImage8U);
	cvReleaseImage(&rgbImage32F);
	cvReleaseImage(&labels8U);
	cvReleaseImage(&hlsImage8U);
	cvReleaseImage(&hlsImage32F);
}

void Test::Run15() {
	JarvisMarch *jarvisMarch = new JarvisMarch();
	vector<int> *rows = new vector<int>();
	vector<int> *cols = new vector<int>();
	rows->push_back(1); cols->push_back(1);
	rows->push_back(1);	cols->push_back(2);
	rows->push_back(1);	cols->push_back(3);
	rows->push_back(1);	cols->push_back(4);
	rows->push_back(2);	cols->push_back(1);
	rows->push_back(3);	cols->push_back(1);
	rows->push_back(4);	cols->push_back(1);
	rows->push_back(2);	cols->push_back(2);
	rows->push_back(3);	cols->push_back(3);
	rows->push_back(4);	cols->push_back(4);
	rows->push_back(5);	cols->push_back(5);
	rows->push_back(2);	cols->push_back(4);
	rows->push_back(4);	cols->push_back(6);
	rows->push_back(6);	cols->push_back(3);
	rows->push_back(7);	cols->push_back(2);
	rows->push_back(100); cols->push_back(5);
	rows->push_back(80); cols->push_back(6);
	//for (int i=0;i<rows->size();i++) {
	//	cout<<(*rows)[i]<<","<<(*cols)[i]<<";  ";
	//}
	/*vector<int> *rightRows = new vector<int>();
	vector<int> *rightCols = new vector<int>();
	vector<int> *leftRows = new vector<int>();
	vector<int> *leftCols = new vector<int>();
	int maxRow = 0, maxCol = 0;*/
	//QuickHull::divide(&rows, &cols, 1, 1, 2, 1, false, maxRow, maxCol);
	//for (int i=0;i<(rows)->size();i++) {
	//	cout<<"r:"<<(*rows)[i]<<" c:"<<(*cols)[i]<<endl;
	//}
	//cout<<"maxRow: "<<maxRow<<" maxCol: "<<maxCol<<endl;
	//vector<ContourPixel *> *conts = new vector<ContourPixel *>();
	//QuickHull::findHull(rows, cols, conts, 1, 1, 5, 5, true);

	//QuickHull::divide(rows, cols, rightRows, rightCols, 5, 5, 1, 1, false);
	vector<ContourPixel *> *conts = new vector<ContourPixel *>();
	jarvisMarch->findConvexHull(rows, cols, conts, 4);
	//vector<ContourPixel *> *conts = QuickHull::contourPixels(rows, cols, 1, 1, 100, 5, 8);
	for (int i=0;i<conts->size();i++) {
		ContourPixel *pixel = (*conts)[i];
		cout<<pixel->getRow()<<" "<<pixel->getCol()<<endl;
	}
	

}

void Test::drawBlob(IplImage *image, Blob *blob, int color) {
	int area = blob->getArea();
	vector<int> *rows = blob->getRowPixels();
	vector<int> *cols = blob->getColumnPixels();
	uchar *data = (uchar *)image->imageData;
	for (int i=0;i<area;i++) {
		int row = (*rows)[i];
		int col = (*cols)[i];
		data[row*image->widthStep+col*image->nChannels] = color;
	}
	float meanChan1 = blob->getChan1();
	float meanChan2 = blob->getChan2();
	float meanChan3 = blob->getChan3();
	cout<<"Blob area: "<<blob->getArea();
	if (blob->isSkinBlob())
		cout<<"!!!SKIN BLOB!!!";
	cout<<"skin hits: "<<blob->skinHits;
	cout<<endl;
}

void Test::drawBlobs(IplImage *image, vector<Blob *> *blobVector) {
	cout<<"total blobs: "<<blobVector->size()<<endl;
	uchar *data = (uchar *)image->imageData;
	for (int i=1;i<blobVector->size();i++) {
		Blob *blob = (*blobVector)[i];
		vector<int> *rowPixels = blob->getRowPixels();
		vector<int> *columnPixels = blob->getColumnPixels();
		for (int j=0;j<rowPixels->size();j++) {
			int row = (*rowPixels)[j];
			int col = (*columnPixels)[j];
			data[row*image->widthStep+col*image->nChannels] = blob->getLabel();
			//cout<<"blob drawn at: "<<row<<" "<<col<<endl;
		}
	}
}

void Test::drawBlobAndNeighbours(IplImage *image, Blob *blob, int color) {
	vector <Blob *> *neighbours = blob->getAdjacentBlobs();
	drawBlob(image, blob, color);
	cout<<"THIS blob id: ";
	blob->printID();
	cout<<endl;
	cout<<"THIS blob color: ";
	blob->printMeanColor();
	cout<<endl;
	cout<<"Number of neighbours: "<<neighbours->size()<<endl;
	for (int i=0;i<neighbours->size();i++) {
		Blob *nBlob = (*neighbours)[i];
		if ((nBlob->getID()->getR() == 0) && (nBlob->getID()->getG() == 0) && (nBlob->getID()->getB() == 1))
			cout<<"Background blob is a neighbour as well"<<endl;
		drawBlob(image, nBlob, color + 30);
		drawCentroid(image, nBlob, 255);
		//drawContours(image, nBlob, 255);
		cout<<"Neighbour ID: ";
		nBlob->printID();
		cout<<" label: "<<nBlob->getLabel();
		cout<<" size: "<<nBlob->getArea()<<endl;
	}
}

void Test::printColor(IplImage *image, int row, int col) {
	uchar *data = (uchar *)image->imageData;
	int color = data[row*image->widthStep+col*image->nChannels];
	cout<<"Color at: "<<row<<","<<col<<" is: "<<color<<endl;
	
}

void Test::printValue32F(IplImage *image, int row, int col) {
	float chan1 = ((float *)(image->imageData + row*image->widthStep))[col*image->nChannels + 0];
	float chan2 = ((float *)(image->imageData + row*image->widthStep))[col*image->nChannels + 1];
	float chan3 = ((float *)(image->imageData + row*image->widthStep))[col*image->nChannels + 2];
	cout<<"Value at: "<<row<<","<<col<<" is: "<<chan1<<" "<<chan2<<" "<<chan3<<endl;
}

void Test::printValue8U(IplImage *image, int row, int col) {
	uchar *data = (uchar *)image->imageData;
	int chan1 = data[row*image->widthStep+col*image->nChannels+0];
	int chan2 = data[row*image->widthStep+col*image->nChannels+1];
	int chan3 = data[row*image->widthStep+col*image->nChannels+2];
	cout<<"Value at: "<<row<<","<<col<<" is: "<<chan1<<" "<<chan2<<" "<<chan3<<endl;
}

void Test::drawCentroid(IplImage *image, Blob *blob, int color) {
	int row = blob->getCentroidRow();
	int col = blob->getCentroidCol();
	cvCircle(image, cvPoint(col, row), 3, cvScalar(color));
	//cout<<"Centroid at row:"<<row<<", col:"<<col<<endl;

}

void Test::drawContours(IplImage *image, Blob *blob, int color) {
	vector<ContourPixel *> *contourPixels = blob->getContourPixels();
	for (int i=0;i<contourPixels->size();i++) {
		ContourPixel *contourPixel = (*contourPixels)[i];
		cvCircle(image, cvPoint(contourPixel->getCol(), contourPixel->getRow()), 2, cvScalar(color));
	}
}